#include "foc.h"
#include "arm_math.h"
#include "stdio.h"


#define FOC_ANGLE_TO_RADIN	0.01745f	// 角度转换为弧度（PI/180）
#define FOC_SQRT_3 			1.73205f	// 根号3


static void GetElectricalAngle(PFOC_Struct pFOC);
static void CurrentReconstruction(PFOC_Struct pFOC);
static void ClarkeTransform(PFOC_Struct pFOC);
static void ParkTransform(PFOC_Struct pFOC);
static void CurrentPIControlID(PFOC_Struct pFOC);
static void CurrentPIControlIQ(PFOC_Struct pFOC);
static void ParkAntiTransform(PFOC_Struct pFOC);


/**
 * @brief   FOC/电流/力矩环控制
 *
 * @param   pFOC		需要控制的foc结构体
 *
 * @return  none
 */
void FocContorl(PFOC_Struct pFOC)
{
    // 1. 获取电气角度
    GetElectricalAngle(pFOC);

    // 2. 计算实际电流值
	// 2.1. 电流重构
	CurrentReconstruction(pFOC);
	// 2.2. Clarke变换
	ClarkeTransform(pFOC);
	// 2.3. Park变换
	ParkTransform(pFOC);

	/* 调试信息输出（经过变换之后的dq应该是一个常量） */
	FocPrintf(pFOC);
	
    // 3. 做PID闭环
    CurrentPIControlID(pFOC);
    CurrentPIControlIQ(pFOC);
	
	/* 开环测试打开下面两行 */
//	pFOC->idPID.out = 0.0;
//	pFOC->iqPID.out = 3.0;
	
    // 4. 计算输出值iα i贝塔
    ParkAntiTransform(pFOC);
    // 5. 输出SVPWM
    pFOC->SvpwmGenerate(pFOC->i_alpha_SVPWM, pFOC->i_beta_SVPWM);
	
	return;
}

/**
 * @brief   更新电角度
 *
 * @param   pFOC		需要更新参数的foc结构体
 *
 * @return  none
 */
static void GetElectricalAngle(PFOC_Struct pFOC)
{
	// 1. 获取机械角度
	pFOC->mAngle	= pFOC->GetEncoderAngle();
	// 2. 计算电角度
    pFOC->eAngle	= pFOC->mAngle * pFOC->polePairs;
	while(pFOC->eAngle > 360)
		pFOC->eAngle = pFOC->eAngle - 360;
	// 3. 转换为弧度制
    pFOC->eRadian	= pFOC->eAngle * FOC_ANGLE_TO_RADIN;
}

/**
 * @brief   电流重构
 *
 * @param   pFOC		需要更新参数的foc结构体
 *
 * @return  none
 */
static void CurrentReconstruction(PFOC_Struct pFOC)
{
	// 1. 更新三相电流
    pFOC->GetPreCurrent(&pFOC->ia, &pFOC->ib, &pFOC->ic);
	
	// 2. 判断电流采集项数（只采集了两项就不用重构了）
	if (pFOC->iNum < 3)
        return;
	
	// 3. 根据扇区重构电流
    switch (pFOC->GetSVPWMSector())
	{
        case 1:
            pFOC->ia = 0.0f - pFOC->ib - pFOC->ic;
            break;
        case 2:
            pFOC->ib = 0.0f - pFOC->ia - pFOC->ic;
            break;
        case 3:
            pFOC->ib = 0.0f - pFOC->ia - pFOC->ic;
            break;
        case 4:
            pFOC->ic = 0.0f - pFOC->ia - pFOC->ib;
            break;
        case 5:
            pFOC->ic = 0.0f - pFOC->ia - pFOC->ib;
            break;
        case 6:
            pFOC->ia = 0.0f - pFOC->ib - pFOC->ic;
            break;
        default:
            break;
    }
}

/**
 * @brief   clarke变换函数
 *
 * @param   pFOC		需要更新参数的foc结构体
 *
 * @return  none
 */
static void ClarkeTransform(PFOC_Struct pFOC)
{
    pFOC->i_alpha = pFOC->ia;
    pFOC->i_beta  = (pFOC->ia + 2.0f * pFOC->ib) / FOC_SQRT_3;
}

/**
 * @brief   park变换函数
 *
 * @param   pFOC		需要更新参数的foc结构体
 *
 * @return  none
 */
static void ParkTransform(PFOC_Struct pFOC)
{
    pFOC->id =  pFOC->i_alpha * arm_cos_f32(pFOC->eRadian) + pFOC->i_beta * arm_sin_f32(pFOC->eRadian);
    pFOC->iq = -pFOC->i_alpha * arm_sin_f32(pFOC->eRadian) + pFOC->i_beta * arm_cos_f32(pFOC->eRadian);
}

/**
 * @brief   电流环id向量PI控制
 *
 * @param   pFOC		需要更新参数的foc结构体
 *
 * @return  none
 */
static void CurrentPIControlID(PFOC_Struct pFOC)
{
//	float p, i, d;
	
    // 1. 获取实际值
    pFOC->idPID.pre = pFOC->id ;
    // 2. 获取目标值
    pFOC->idPID.tar = pFOC->tarid;
    // 3. 计算偏差
    pFOC->idPID.bias = pFOC->idPID.tar - pFOC->idPID.pre;
	
    // 4. 计算PID输出值
	
	/* 以下是：ODrive原版的PID算法：增量式(第4步只有下面这一行) */
    pFOC->idPID.out += pFOC->idPID.kp * (pFOC->idPID.bias - pFOC->idPID.lastBias) + pFOC->idPID.ki * pFOC->idPID.bias;
    /* 以上是：ODrive原版的PID算法：增量式(第4步只有上面这一行) */
	
	/* 以下是：修改后的PID算法：位置式 */
//	// 计算pid的输出
//	p = pFOC->idPID.kp * pFOC->idPID.bias;
//	i = pFOC->idPID.ki * pFOC->idPID.integral;
//	d = pFOC->idPID.kd * (pFOC->idPID.bias - pFOC->idPID.lastBias);
//    // 积分限幅
//	if (i > fabs(pFOC->idPID.outMax))
//	{
//        i = fabs(pFOC->idPID.outMax);
//    }
//    if (i < -fabs(pFOC->idPID.outMax))
//	{
//        i = -fabs(pFOC->idPID.outMax);
//    }
//	// 更新积分
//	pFOC->idPID.integral += i;
//	// 计算输出和
//	pFOC->idPID.out = p + i + d;
	/* 以上是：修改后的PID算法：位置式 */
	
	// 5. 保存偏差
    pFOC->idPID.lastBias = pFOC->idPID.bias;
	// 6. 限幅
    if (pFOC->idPID.out > fabs(pFOC->idPID.outMax))
	{
        pFOC->idPID.out = fabs(pFOC->idPID.outMax);
    }
    if (pFOC->idPID.out < -fabs(pFOC->idPID.outMax))
	{
        pFOC->idPID.out = -fabs(pFOC->idPID.outMax);
    }
}

/**
 * @brief   电流环iq向量PI控制
 *
 * @param   pFOC		需要更新参数的foc结构体
 *
 * @return  none
 */
static void CurrentPIControlIQ(PFOC_Struct pFOC)
{
//	float p, i, d;
	
    // 1. 获取实际值
    pFOC->iqPID.pre = pFOC->iq;
    // 2. 获取目标值
    pFOC->iqPID.tar = pFOC->tariq;
    // 3. 计算偏差
    pFOC->iqPID.bias = pFOC->iqPID.tar - pFOC->iqPID.pre;
    // 4. 计算PID输出值
    
	/* 以下是：ODrive原版的PID算法：增量式(第4步只有下面这一行) */
    pFOC->iqPID.out += pFOC->iqPID.kp * (pFOC->iqPID.bias - pFOC->iqPID.lastBias) + pFOC->iqPID.ki * pFOC->iqPID.bias;
    /* 以上是：ODrive原版的PID算法：增量式(第4步只有下面这一行) */
	
	/* 以下是：修改后的PID算法：位置式 */
//	// 计算pid的输出
//	p = pFOC->iqPID.kp * pFOC->iqPID.bias;
//	i = pFOC->iqPID.ki * pFOC->iqPID.integral;
//	d = pFOC->iqPID.kd * (pFOC->iqPID.bias - pFOC->iqPID.lastBias);
//    // 积分限幅
//	if (i > fabs(pFOC->iqPID.outMax))
//	{
//        i = fabs(pFOC->iqPID.outMax);
//    }
//    if (i < -fabs(pFOC->iqPID.outMax))
//	{
//        i = -fabs(pFOC->iqPID.outMax);
//    }
//	// 更新积分
//	pFOC->iqPID.integral += i;
//	// 计算输出和
//	pFOC->iqPID.out = p + i + d;
	/* 以上是：修改后的PID算法：位置式 */
	
	// 5. 保存偏差
    pFOC->iqPID.lastBias = pFOC->iqPID.bias;
	// 6. 限幅
    if (pFOC->iqPID.out > fabs(pFOC->iqPID.outMax))
	{
        pFOC->iqPID.out = fabs(pFOC->iqPID.outMax);
    }
    if (pFOC->iqPID.out < -fabs(pFOC->iqPID.outMax))
	{
        pFOC->iqPID.out = -fabs(pFOC->iqPID.outMax);
    }
}

/**
 * @brief   park反变换函数
 *
 * @param   pFOC		需要更新参数的foc结构体
 *
 * @return  none
 */
static void ParkAntiTransform(PFOC_Struct pFOC)
{
    pFOC->i_alpha_SVPWM = pFOC->idPID.out * arm_cos_f32(pFOC->eRadian) - pFOC->iqPID.out * arm_sin_f32(pFOC->eRadian);
    pFOC->i_beta_SVPWM  = pFOC->idPID.out * arm_sin_f32(pFOC->eRadian) + pFOC->iqPID.out * arm_cos_f32(pFOC->eRadian);
}

/* ============================================================= */

/**
 * @brief   设置电流/力矩/FOC环目标值
 *
 * @param   pFOC		需要更新参数的foc结构体
 * @param   tarid		d轴目标值
 * @param   tariq		q轴目标值
 *
 * @return  none
 */
void SetCurrentPIDTar(PFOC_Struct pFOC, float tarid, float tariq)
{
    pFOC->tarid = tarid;
    pFOC->tariq = tariq;
}

/**
 * @brief   设置电流/力矩/FOC环 PID控制器的参数
 *
 * @param   pFOC		需要更新参数的foc结构体
 * @param   kp			比例系数
 * @param   ki			积分系数
 * @param   kd			微分系数
 * @param   outMax		输出限幅
 *
 * @return  none
 */
void SetCurrentPIDParams(PFOC_Struct pFOC, float kp, float ki, float kd, float outMax)
{
    pFOC->idPID.kp = kp;
    pFOC->idPID.ki = ki;
    pFOC->idPID.kd = kd;
    pFOC->idPID.outMax = outMax;

    pFOC->iqPID.kp = kp;
    pFOC->iqPID.ki = ki;
    pFOC->iqPID.kd = kd;
    pFOC->iqPID.outMax = outMax;
}

/**
 * @brief   设置FOC使能
 *
 * @param   pFOC		需要更新参数的foc结构体
 * @param   isEnable	0：失能；1：使能
 *
 * @return  none
 */
void SetFocEnable(PFOC_Struct pFOC, uint8_t isEnable)
{
    pFOC->isEnable = isEnable;
	pFOC->SetEnable(pFOC->isEnable);
}

/**
 * @brief   输出FOC控制信息
 *
 * @param   pFOC		需要更新参数的foc结构体
 *
 * @return  none
 */
#include "vofa.h"
void FocPrintf(PFOC_Struct pFOC)
{
	float ch_data[7];
	
	// 调试信息输出
	ch_data[0] = pFOC->iq;
	ch_data[1] = pFOC->tariq;
	
	ch_data[2] = pFOC->id;
	ch_data[3] = pFOC->tarid;
	
	ch_data[4] = pFOC->idPID.kp;
	ch_data[5] = pFOC->idPID.ki;
	ch_data[6] = pFOC->idPID.kd;
	
//	ch_data[7] = pFOC->ia;
//	ch_data[8] = pFOC->ib;
//	ch_data[9] = pFOC->ic;
	VOFA_JUSTFLOAT_SEND(ch_data, sizeof(ch_data)/sizeof(float));
}

/**
 * @brief   输出电机机械角度
 *
 * @param   pFOC		foc结构体
 *
 * @return  电机的机械角度
 */
float GetFocAngle(PFOC_Struct pFOC)
{
	return pFOC->mAngle;
}

